//----------------------------------------------------------------------------
// M A R I T I M E  R E S E A R C H  I N S T I T U T E  N E T H E R L A N D S
//----------------------------------------------------------------------------
// Copyright (C) 2008 - MARIN - All rights reserved - http://www.marin.nl
//----------------------------------------------------------------------------
// Program    : mermaid
// Module     : vbm
// File       : RrbAkashSolver.cpp
//----------------------------------------------------------------------------

#include <math.h>
#include "RrbAkashSolver.h"


//#include "FloatUtils.h"
//#include "Timer.h"

//#include "omp.h"
/*
extern int Nx1, Nx2;
#define COLUMNS  (unsigned)(Nx1 + 3)
#define ROWS     (unsigned)(Nx2 + 3)
#define COLUMNS2 (2 * (Nx1 + 2) + 1)
#define ROWS2    (2 * (Nx2 + 2) + 1)
*/

// these may not be member variables because they are used in openmp reductions
//static REAL rhoold = 0.0;
//static REAL rhonew = 0.0;
//static REAL pq     = 0.0;

/// constructor
RrbAkashSolver::RrbAkashSolver(const Array2D<double> &pzzcc,
                               const Array2D<double> &pzzcs,
                               const Array2D<double> &pzzcw,
                               const Array2D<double> &pffcc,
                               const Array2D<double> &pffcs,
                               const Array2D<double> &pffcw,
                               const Array2D<double> &pzfcc,
                               const Array2D<double> &pzfcs,
                               const Array2D<double> &pzfcw,
                               const Array2D<double> &pfzcc,
                               const Array2D<double> &pfzcs,
                               const Array2D<double> &pfzcw,
                               int Nx1,
                               int Nx2,
                               REAL tolerance)
    : m_Nx1(Nx1)
    , m_Nx2(Nx2)
    , COLUMNS(m_Nx1+3)
    , ROWS(m_Nx2+3)
    , m_tolerance(tolerance)
    , m_pzzcc(pzzcc)
    , m_pzzcs(pzzcs)
    , m_pzzcw(pzzcw)
    , m_pffcc(pffcc)
    , m_pffcs(pffcs)
    , m_pffcw(pffcw)
    , m_pzfcc(pzfcc)
    , m_pzfcs(pzfcs)
    , m_pzfcw(pzfcw)
    , m_pfzcc(pfzcc)
    , m_pfzcs(pfzcs)
    , m_pfzcw(pfzcw)
    /*
                        , m_czzne(COLUMNS, ROWS)
                        , m_czznw(COLUMNS, ROWS)
                        , m_czzse(COLUMNS, ROWS)
                        , m_czzsw(COLUMNS, ROWS)

                        , m_cffne(COLUMNS, ROWS)
                        , m_cffnw(COLUMNS, ROWS)
                        , m_cffse(COLUMNS, ROWS)
                        , m_cffsw(COLUMNS, ROWS)

                        , m_czzcc(COLUMNS, ROWS)
                        , m_czzss(COLUMNS, ROWS)
                        , m_czzww(COLUMNS, ROWS)
                        , m_czzee(COLUMNS, ROWS)
                        , m_czznn(COLUMNS, ROWS)

                        , m_cffcc(COLUMNS, ROWS)
                        , m_cffss(COLUMNS, ROWS)
                        , m_cffww(COLUMNS, ROWS)
                        , m_cffee(COLUMNS, ROWS)
                        , m_cffnn(COLUMNS, ROWS)
                    */
    , m_st(0)
    , m_tw(0)
    , m_niv(0)
    , m_rhobew(0.0)
    , m_bet(0.0)
    , m_alf(0.0)
    , m_done(false)
    , m_done1(false)
    , m_done2(false)
{
    //  ASSERT(pcc->dim1() == COLUMNS && pcc->dim2() == ROWS, "c array size mismatch");
    //  ASSERT(pcs->dim1() == COLUMNS && pcs->dim2() == ROWS, "s array size mismatch");
    //  ASSERT(pcw->dim1() == COLUMNS && pcw->dim2() == ROWS, "w array size mismatch");
    // Construct Preconditioner for S_ZetaZeta


    storematrices();
    preconditioner(m_ZetaZeta, &m_pZetaZeta);
    preconditioner(m_PhiPhi, &m_pPhiPhi);


    // Check the Matrix Vector Multiplication
    /* Array2D<REAL> test(COLUMNS, ROWS);
    test=2.5;

    test = matvec(m_ZetaPhi,test);
    PrintArray2Dcontents1D("",test,0,test.dim1()-1,0,test.dim2());


    for (int i =0; i<m_PhiPhi.size(); ++i)
    {
        //std::string s = "Z" + i;
        //   PrintArray2Dcontents1D(s,m_PhiPhi[i],1,Nx1-1,1,Nx2-1);
    }

    std::cout << m_ZetaPhi.size() << std::endl;
    for (int i =0; i<m_ZetaPhi.size(); ++i)
    {
         std::string s = "Z" + std::to_string(i);
          PrintArray2Dcontents1D(s,m_ZetaPhi[i],1,Nx1,1,Nx2)
    }
*/

}

/// destructor
RrbAkashSolver::~RrbAkashSolver()
{
    //#ifdef spy
    //    PrintArray2Dcontents("", m_cne, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_cnw, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_cse, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_csw, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_ccc, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_css, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_cww, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_cee, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //    PrintArray2Dcontents("", m_cnn, 0, m_Nx1 + 1, 0, m_Nx2 + 1);
    //
    //    PlotArrayContents("", m_cne, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_cnw, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_cse, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_csw, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_ccc, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_css, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_cww, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_cee, 1, m_Nx1, 1, m_Nx2);
    //    PlotArrayContents("", m_cnn, 1, m_Nx1, 1, m_Nx2);
    //#endif
}


// AM: Store matrix Diagonals in the vector form


void RrbAkashSolver::storematrices()
{

    // Store the matrix of S_ZetaZeta
    Array2D<REAL> cc = m_pzzcc;
    Array2D<REAL> cs = m_pzzcs;
    Array2D<REAL> cw = m_pzzcw;
    Array2D<REAL> cn = cs;
    Array2D<REAL> ce = cw;

    for (int jj = 0; jj <= m_Nx2 + 1; ++jj)
    {
        for (int ii = 0; ii <= m_Nx1 + 1; ++ii)
        {
            cn[ii][jj] = -cs[ii ][jj + 1]; // Make use of the fact that the matrix is Skew-Symmetric.
            ce[ii][jj] = -cw[ii + 1][jj ]; // Make use of the fact that the matrix is Skew-Symmetric.
        }
    }

    m_ZetaZeta.push_back(cc);
    m_ZetaZeta.push_back(cs);
    m_ZetaZeta.push_back(cw);
    m_ZetaZeta.push_back(cn);
    m_ZetaZeta.push_back(ce);


    // Store the matrix of S_PhiPhi
    cc = m_pffcc;
    cs = m_pffcs;
    cw = m_pffcw;
    cn = cs;
    ce = cw;

    for (int jj = 0; jj <= m_Nx2 + 1; ++jj)
    {
        for (int ii = 0; ii <= m_Nx1 + 1; ++ii)
        {
            cn[ii][jj] = -cs[ii ][jj + 1]; // Make use of the fact that the matrix is Skew-Symmetric.
            ce[ii][jj] = -cw[ii + 1][jj ]; // Make use of the fact that the matrix is Skew-Symmetric.
        }
    }

    m_PhiPhi.push_back(cc);
    m_PhiPhi.push_back(cs);
    m_PhiPhi.push_back(cw);
    m_PhiPhi.push_back(cn);
    m_PhiPhi.push_back(ce);



    // AM: Store the Array in the Vector Form for S_zeta_phi
    cc = m_pzfcc;
    cs = m_pzfcs;
    cw = m_pzfcw;
    cn = cs;
    ce = cw;

    for (int jj = 0; jj <= m_Nx2 + 1; ++jj)
    {
        for (int ii = 0; ii <= m_Nx1 + 1; ++ii)
        {
            cn[ii][jj] = cs[ii ][jj + 1]; // Make use of the fact that the matrix is Symmetric.
            ce[ii][jj] = cw[ii + 1][jj ]; // Make use of the fact that the matrix is Symmetric.
        }
    }

    m_ZetaPhi.push_back(cc);
    m_ZetaPhi.push_back(cs);
    m_ZetaPhi.push_back(cw);
    m_ZetaPhi.push_back(cn);
    m_ZetaPhi.push_back(ce);

    // AM: Store the Array in the Vector Form for S_phi_zeta  -- Later need to be modified, as this has only one center column
    cc = m_pfzcc;
    cs = m_pfzcs;
    cw = m_pfzcw;
    cn = cs;
    ce = cw;
    for (int jj = 0; jj <= m_Nx2 + 1; ++jj)
    {
        for (int ii = 0; ii <= m_Nx1 + 1; ++ii)
        {
            cn[ii][jj] = cs[ii ][jj + 1]; // Make use of the fact that the matrix is Symmetric.
            ce[ii][jj] = cw[ii + 1][jj ]; // Make use of the fact that the matrix is Symmetric.
        }
    }


    m_PhiZeta.push_back(cc);
    m_PhiZeta.push_back(cs);
    m_PhiZeta.push_back(cw);
    m_PhiZeta.push_back(cn);
    m_PhiZeta.push_back(ce);
}



/// preconditioner
/** Deze subroutine maakt een LDL'-ontbinding van een symmetrische, pos. sem. 
    definiete matrix volgens de RRB-methode. \n
    L' en D worden opgeslagen in stencil-vorm.
    L' wordt in opgeslagen in de




 arrays cnn, css, cee en cww. \n
    Omdat het nieuwe Schurcomplement telkens gerepresenteerd kan worden als een
    negen-punts stencil, hebben we tijdens de incomplete ontbinding als kladarrays
    nodig: cse, csw, cne en cnw. \n
    Het laatste stuk wordt exact gedaan. Het laatste Schur-complement heeft
    grootte sg en bandbreedte bsm. Een incomplete ontbinding van het laatste
    Schur-complement wordt opgeslagen in s.  */

/** This subroutine creates a LDL' factorization of a symmetric, pos. sem.
     definite matrix according to the RRB method. \ n
     L 'and D are stored in stencil form.
     L 'is stored in the arrays cnn, css, cee and cww. \ n
     Because the new Schur Complement can be represented as a constantly
     nine-point stencil, we have during the incomplete dissolution as a draft arrays
     needed: cse, CSW, cne and cnw. \ n
     The last piece is done exactly. The last Schur complement has
     sg size and bandwidth bsm. An incomplete dissolution of the last
     Schur complement is stored in s. */

void RrbAkashSolver::preconditioner(const std::vector<Array2D<REAL> > &mat_st,
                                    std::vector<Array2D<REAL> > *mat_pc)
{



    Array2D<REAL> m_ccc = mat_st[0];
    Array2D<REAL> m_css = mat_st[1];
    Array2D<REAL> m_cww = mat_st[2];
    Array2D<REAL> m_cnn = mat_st[3];
    Array2D<REAL> m_cee = mat_st[4];


    Array2D<REAL> m_cne(COLUMNS, ROWS);
    Array2D<REAL> m_cnw(COLUMNS, ROWS);
    Array2D<REAL> m_cse(COLUMNS, ROWS);
    Array2D<REAL> m_csw(COLUMNS, ROWS);

    m_cne=0;
    m_cnw=0;
    m_cse=0;
    m_csw=0;

    int ii=0;
    int jj=0;
    int st = 1;
    int tw = 2;
    int niv = 0;
    while (tw < m_Nx2 + 2)
    {
        //                                 cnw cnn cne
        // Ga uit van een 9-punts stencil  cww ccc cee  / / Assume a 9-point stencil cww ccc cee
        //                                 csw css cse
        // Doorloop eerst een schaakbord met daarin cnw, cne, ccc, csw en cse / / Iterate first chessboard containing cnw, cne, ccc, CSW and cse
        // als zwarte punten in S11. Lump cnw, cne, csw en cse op de rest.  / / As black dots in S11. Lump cnw, cne, CSW and cse the rest.
        // Dit geeft een 5-punts stencil en diagonaalmatrix S11. / / This gives a 5-point stencil and diagonal matrix S11.
        //                                     cnn
        //                                 cww ccc cee
        //                                     css
        if (niv > 0)
        {
            REAL fac = 1.0 + st * 10.0 / ((m_Nx1 + 2) * (m_Nx2 + 2));
            for (jj = st; jj <= m_Nx2 + 2; jj += tw)
            {
                for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
                {
                    m_ccc[ii][jj] = m_ccc[ii][jj] * fac +
                            m_cne[ii][jj] + m_cnw[ii][jj] +
                            m_cse[ii][jj] + m_csw[ii][jj];
                }
            }
            for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
            {
                for (ii = st; ii <= m_Nx1 + 2; ii += tw)
                {
                    m_ccc[ii][jj] = m_ccc[ii][jj] * fac +
                            m_cne[ii][jj] + m_cnw[ii][jj] +
                            m_cse[ii][jj] + m_csw[ii][jj];
                }
            }
        }
        ++niv;


        // Voor de overblijvende witte punten hebben we eveneens
        //                      cnw cnn cne
        // een 9-punts stencil  cww ccc cee
        //                      csw css cse
        // met dus cnw, cne, ccc, csw en cse als witte punten.
        // Op de witte punten denken we een nieuw schaakbord aangebracht =>
        // S22 en S33. We elimineren nu alle zwarte punten met de zojuist
        // afgeleide 5-punts formule voor de zwarte punten. We krijgen dan
        //                          cnn
        //                      cnw     cne
        //                  cww     ccc     cee
        //                      csw     cse
        //                          css
        for (jj = st; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = st; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2+2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cnn[ii][jj] * m_cee[ii][jn] / m_ccc[ii][jn];
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cnn[ii][jj] * m_cww[ii][jn] / m_ccc[ii][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnn[ii][jj] * m_css[ii][jn] / m_ccc[ii][jn];
                    m_cnn[ii][jj] = -m_cnn[ii][jj] * m_cnn[ii][jn] / m_ccc[ii][jn];
                }
                if (js > 0)
                {
                    m_cse[ii][jj] = m_cse[ii][jj] - m_css[ii][jj] * m_cee[ii][js] / m_ccc[ii][js];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_css[ii][jj] * m_cww[ii][js] / m_ccc[ii][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_css[ii][jj] * m_cnn[ii][js] / m_ccc[ii][js];
                    m_css[ii][jj] = -m_css[ii][jj] * m_css[ii][js] / m_ccc[ii][js];
                }
                if (ie <= m_Nx1+2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cee[ii][jj] * m_cnn[ie][jj] / m_ccc[ie][jj];
                    m_cse[ii][jj] = m_cse[ii][jj] - m_cee[ii][jj] * m_css[ie][jj] / m_ccc[ie][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cee[ii][jj] * m_cww[ie][jj] / m_ccc[ie][jj];
                    m_cee[ii][jj] = -m_cee[ii][jj] * m_cee[ie][jj] / m_ccc[ie][jj];
                }
                if (iw > 0)
                {
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cww[ii][jj] * m_cnn[iw][jj] / m_ccc[iw][jj];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_cww[ii][jj] * m_css[iw][jj] / m_ccc[iw][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cww[ii][jj] * m_cee[iw][jj] / m_ccc[iw][jj];
                    m_cww[ii][jj] = -m_cww[ii][jj] * m_cww[iw][jj] / m_ccc[iw][jj];
                }
            }
        }
        for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2+2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cnn[ii][jj] * m_cee[ii][jn] / m_ccc[ii][jn];
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cnn[ii][jj] * m_cww[ii][jn] / m_ccc[ii][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnn[ii][jj] * m_css[ii][jn] / m_ccc[ii][jn];
                    m_cnn[ii][jj] = -m_cnn[ii][jj] * m_cnn[ii][jn] / m_ccc[ii][jn];
                }
                if (js > 0)
                {
                    m_cse[ii][jj] = m_cse[ii][jj] - m_css[ii][jj] * m_cee[ii][js] / m_ccc[ii][js];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_css[ii][jj] * m_cww[ii][js] / m_ccc[ii][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_css[ii][jj] * m_cnn[ii][js] / m_ccc[ii][js];
                    m_css[ii][jj] = -m_css[ii][jj] * m_css[ii][js] / m_ccc[ii][js];
                }
                if (ie <= m_Nx1+2)
                {
                    m_cne[ii][jj] = m_cne[ii][jj] - m_cee[ii][jj] * m_cnn[ie][jj] / m_ccc[ie][jj];
                    m_cse[ii][jj] = m_cse[ii][jj] - m_cee[ii][jj] * m_css[ie][jj] / m_ccc[ie][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cee[ii][jj] * m_cww[ie][jj] / m_ccc[ie][jj];
                    m_cee[ii][jj] = -m_cee[ii][jj] * m_cee[ie][jj] / m_ccc[ie][jj];
                }
                if (iw > 0)
                {
                    m_cnw[ii][jj] = m_cnw[ii][jj] - m_cww[ii][jj] * m_cnn[iw][jj] / m_ccc[iw][jj];
                    m_csw[ii][jj] = m_csw[ii][jj] - m_cww[ii][jj] * m_css[iw][jj] / m_ccc[iw][jj];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cww[ii][jj] * m_cee[iw][jj] / m_ccc[iw][jj];
                    m_cww[ii][jj] = -m_cww[ii][jj] * m_cww[iw][jj] / m_ccc[iw][jj];
                }
            }
        }

        // Voor de overblijvende witte punten hebben we
        //                          cnn
        //                      cnw     cne
        //                   cww    ccc    cee
        //                      csw     cse
        //                          css
        // Hierin lumpen we de locale coefficienten cnloc etc. op de rest
        //  => scheve 5-punts formule
        //                      cnw     cne
        //                          ccc
        //                      csw     cse
        for (jj = st; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = st; ii <= m_Nx1 + 2; ii += tw)
            {
                m_ccc[ii][jj] = m_ccc[ii][jj] +
                        m_cnn[ii][jj] +
                        m_cee[ii][jj] +
                        m_css[ii][jj] +
                        m_cww[ii][jj];

                // Om geheugen te besparen gaan we cnw(ii,jj) opslaan in cnn(ii,jj);
                //                                 cne(ii,jj) opslaan in cee(ii,jj);
                //                                 csw(ii,jj) opslaan in cww(ii,jj);
                //                                 cse(ii,jj) opslaan in css(ii,jj);
                m_cnn[ii][jj] = m_cnw[ii][jj];
                m_cee[ii][jj] = m_cne[ii][jj];
                m_cww[ii][jj] = m_csw[ii][jj];
                m_css[ii][jj] = m_cse[ii][jj];
            }
        }
        ++niv;

        // Uitgaande van het stencil
        //                          cnn
        //                     cnw      cne
        //                 cww      ccc      cee
        //                     csw      cse
        //                          css
        // elimineren we cnw en cne met de scheve 5-punts formule. Dit geeft
        //                 cnw      cnn      cne
        //                 cww      ccc      cee
        //                 csw      css      cse
        // We zijn nu weer terug de eerste 9-punts formule, maar op een
        // grover rooster.
        for (jj = 0; jj <= m_Nx2 + 2; jj += tw)
        {
            for (ii = 0; ii <= m_Nx1 + 2; ii += tw)
            {
                int iw = ii - st;
                int ie = ii + st;
                int js = jj - st;
                int jn = jj + st;
                if (jn <= m_Nx2+2  && ie <= m_Nx1+2 )
                {
                    m_cnn[ii][jj] = m_cnn[ii][jj] - m_cne[ii][jj] * m_cnw[ie][jn] / m_ccc[ie][jn];
                    m_cee[ii][jj] = m_cee[ii][jj] - m_cne[ii][jj] * m_cse[ie][jn] / m_ccc[ie][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cne[ii][jj] * m_csw[ie][jn] / m_ccc[ie][jn];
                    m_cne[ii][jj] = -m_cne[ii][jj] * m_cne[ie][jn] / m_ccc[ie][jn];
                }
                if (iw > 0 && jn <= m_Nx2+2 )
                {
                    m_cnn[ii][jj] = m_cnn[ii][jj] - m_cnw[ii][jj] * m_cne[iw][jn] / m_ccc[iw][jn];
                    m_cww[ii][jj] = m_cww[ii][jj] - m_cnw[ii][jj] * m_csw[iw][jn] / m_ccc[iw][jn];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cnw[ii][jj] * m_cse[iw][jn] / m_ccc[iw][jn];
                    m_cnw[ii][jj] = -m_cnw[ii][jj] * m_cnw[iw][jn] / m_ccc[iw][jn];
                }
                if (js > 0 && iw > 0)
                {
                    m_css[ii][jj] = m_css[ii][jj] - m_csw[ii][jj] * m_cse[iw][js] / m_ccc[iw][js];
                    m_cww[ii][jj] = m_cww[ii][jj] - m_csw[ii][jj] * m_cnw[iw][js] / m_ccc[iw][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_csw[ii][jj] * m_cne[iw][js] / m_ccc[iw][js];
                    m_csw[ii][jj] = -m_csw[ii][jj] * m_csw[iw][js] / m_ccc[iw][js];
                }
                if (js > 0 && ie <= m_Nx1+2 )
                {
                    m_css[ii][jj] = m_css[ii][jj] - m_cse[ii][jj] * m_csw[ie][js] / m_ccc[ie][js];
                    m_cee[ii][jj] = m_cee[ii][jj] - m_cse[ii][jj] * m_cne[ie][js] / m_ccc[ie][js];
                    m_ccc[ii][jj] = m_ccc[ii][jj] - m_cse[ii][jj] * m_cnw[ie][js] / m_ccc[ie][js];
                    m_cse[ii][jj] = -m_cse[ii][jj] * m_cse[ie][js] / m_ccc[ie][js];
                }
            }
        }
        st = tw;
        tw *= 2;
    }

    // Achteraf delen door hoofddiagonaalelement om
    // incomplete LDL'-ontbinding te krijgen:
    for (ii = 0; ii <= m_Nx1 + 2; ++ii)
    {
        for (jj = 0; jj <= m_Nx2 + 2; ++jj)
        {
            m_ccc[ii][jj] = 1.0 / m_ccc[ii][jj];
            m_cnn[ii][jj] = m_cnn[ii][jj] * m_ccc[ii][jj];
            m_css[ii][jj] = m_css[ii][jj] * m_ccc[ii][jj];
            m_cee[ii][jj] = m_cee[ii][jj] * m_ccc[ii][jj];
            m_cww[ii][jj] = m_cww[ii][jj] * m_ccc[ii][jj];
        }
    }

    /*
    // Om deling door nul te voorkomen wordt voor alle witte punten
    // van een schaakbordvolgorde niet de hoofddiagonaal van
    // de matrix opgeslagen, maar 1/(--).
    for (jj = 0; jj <= m_Nx2 + 2; jj += 2)
    {
        for (ii = 1; ii <= m_Nx1 + 2; ii += 2)
        {
            cc[ii][jj] = 1.0 / cc[ii][jj];
        }
    }
    for (jj = 1; jj <= m_Nx2 + 2; jj += 2)
    {
        for (ii = 0; ii <= m_Nx1 + 2; ii += 2)
        {
            cc[ii][jj] = 1.0 / cc[ii][jj];
        }
    }*/

    mat_pc->push_back(m_ccc);
    mat_pc->push_back(m_css);
    mat_pc->push_back(m_cww);
    mat_pc->push_back(m_cnn);
    mat_pc->push_back(m_cee);

    // De complete Choleski-ontbinding van het laatste stuk is nu gemaakt.
} // preconditioner

void RrbAkashSolver::solver(const std::vector<Array2D<REAL>> &mat, Array2D<REAL> *p_cse )
{
    // N.B. in de oorspronkelijke code had m_cse de naam Az
    // Deze member variabele is de enige die in deze routine wordt gewijzigd
    // Eerst oplossen L(Az)=Az. We maken hierbij gebruik van symmetrie.
    // Note in the original code had m_cse called Az
    // This member variable is the only one in this routine is changed
    // First solve L (Az) = Az. We do this by using symmetry.


    // AM: Store the p_cse into m_cse

    Array2D<REAL> &m_cse = *p_cse;

    // AM: Get the Diagonals from mat

    Array2D<REAL> m_ccc = mat[0];
    Array2D<REAL> m_css = mat[1];
    Array2D<REAL> m_cww = mat[2];
    Array2D<REAL> m_cnn = mat[3];
    Array2D<REAL> m_cee = mat[4];


    const int nx1 = m_Nx1 + 1;
    const int nx2 = m_Nx2 + 1;
    const int nx12 = m_Nx1 + 2;
    const int nx22 = m_Nx2 + 2;

    { // -----------------------------------------------------------------------
        m_st = 1;
        m_tw = 2;
        m_niv = 0;
        ASSERT(nx22 >= m_tw, "m_tw < m_Nx2 + 2 : " << m_tw << "is not smaller than" << nx22);
    } // master ----------------------------------------------------------------

    // note: the following is the same piece of code as the piece at the end of the next while loop
    // Phase 1: 19.7.2 Do the forward substitution for the points (ii,jj): Updating r2-nodes using r1-nodes in the Same level
    /*
    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii + m_st][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii - m_st][jj - m_st] -= m_cww[ii][jj] * cse;
            m_cse[ii + m_st][jj + m_st] -= m_cee[ii][jj] * cse;
            m_cse[ii - m_st][jj + m_st] -= m_cnn[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_niv += 2;
        m_st = m_tw;
        m_tw *= 2;
    } // master ----------------------------------------------------------------

    // end of repeated code

    { // -----------------------------------------------------------------------
        m_done1 = m_tw >= nx22;
    }
// master ----------------------------------------------------------------
*/


loop1: // while m_tw < nx22

    if (m_done1)
    {
        goto endloop1;
    }

    { // -----------------------------------------------------------------------
        if (m_st <= nx1)
        {
#pragma omp parallel for
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                REAL cse = m_cse[0][jj];
                m_cse[0 ][jj - m_st] -= m_css[0][jj] * cse;
                m_cse[m_st][jj     ] -= m_cee[0][jj] * cse;
                m_cse[0 ][jj + m_st] -= m_cnn[0][jj] * cse;
            }
        }

        if (m_st <= nx2)
        {
            for (int ii = m_st; ii <= nx1; ii += m_tw)
            {
                REAL cse = m_cse[ii][0];
                m_cse[ii + m_st][0 ] -= m_cee[ii][0] * cse;
                m_cse[ii     ][m_st] -= m_cnn[ii][0] * cse;
                m_cse[ii - m_st][0 ] -= m_cww[ii][0] * cse;
            }
        }
    } // master ---------------------------------------------------------------
#pragma omp parallel for
    for (int ii = m_tw; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii     ][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii + m_st][jj     ] -= m_cee[ii][jj] * cse;
            m_cse[ii     ][jj + m_st] -= m_cnn[ii][jj] * cse;
            m_cse[ii - m_st][jj     ] -= m_cww[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------
#pragma omp parallel for
    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_tw; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii     ][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii + m_st][jj     ] -= m_cee[ii][jj] * cse;
            m_cse[ii     ][jj + m_st] -= m_cnn[ii][jj] * cse;
            m_cse[ii - m_st][jj     ] -= m_cww[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    // note: the following is the same piece of code as the piece preceding this while loop
#pragma omp parallel for    
    for (int ii = m_st; ii <= nx1; ii += m_tw)
    {
        for (int jj = m_st; jj <= nx2; jj += m_tw)
        {
            REAL cse = m_cse[ii][jj];
            m_cse[ii + m_st][jj - m_st] -= m_css[ii][jj] * cse;
            m_cse[ii - m_st][jj - m_st] -= m_cww[ii][jj] * cse;
            m_cse[ii + m_st][jj + m_st] -= m_cee[ii][jj] * cse;
            m_cse[ii - m_st][jj + m_st] -= m_cnn[ii][jj] * cse;
        }
    }
    // for ---------------------------------------------------------------------

    { // -----------------------------------------------------------------------
        m_niv += 2;
        m_st = m_tw;
        m_tw *= 2;
    } // master ----------------------------------------------------------------

    // end of repeated code

    { // -----------------------------------------------------------------------
        if (!m_done1 && m_tw >= nx22)
        {
            m_done1 = true;
        }
    } // critical --------------------------------------------------------------

    if (!m_done1)
    {
        goto loop1;
    }

endloop1:; // while m_tw < nx22

    // Nu oplossen D(Az)=Az:

    // AM: Change: Computation is carried out for all the Nodes instead of just the Red Nodes!
#pragma omp parallel for
    for (int ii = 1; ii <= nx1; ii += 1)
    {
        for (int jj = 1; jj <= nx2; jj += 1)
        {
            m_cse[ii    ][jj    ] *= m_ccc[ii    ][jj    ];
            // m_cse[ii - 1][jj - 1] *= m_ccc[ii - 1][jj - 1];
        }
    }
    // for ---------------------------------------------------------------------

    // Nu oplossen U(Az)=Az:
    { //------------------------------------------------------------------------
        m_done2 = m_st <= 1;  // Change: AM : instead of m_st<=2   .. It is m_st <=1
        // temporary?    } // master ---------------------------------------------------------------

loop2: // while (m_st > 2)
        if (m_done2)
        {
            goto endloop2;
        }

        // Nu oplossen U(Az)=Az:
        // note: the following is the same piece of code as the piece following this while loop

        { //------------------------------------------------------------------------
            m_tw = m_st;
            m_st /= 2;
            m_niv -= 2;
        } // master ---------------------------------------------------------------
#pragma omp parallel for
        for (int ii = m_st; ii <= nx1; ii += m_tw)
        {
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                m_cse[ii][jj] += m_css[ii][jj] * m_cse[ii + m_st][jj - m_st] +
                        m_cww[ii][jj] * m_cse[ii - m_st][jj - m_st] +
                        m_cee[ii][jj] * m_cse[ii + m_st][jj + m_st] +
                        m_cnn[ii][jj] * m_cse[ii - m_st][jj + m_st];
            }
        }
        // for ---------------------------------------------------------------------

        // end of repeated code

        { //------------------------------------------------------------------------
            if (m_st <= nx1)
            {
#pragma omp parallel for
                for (int jj = m_st; jj <= nx2; jj += m_tw)
                {
                    m_cse[0][jj] += m_css[0][jj] * m_cse[0 ][jj - m_st] +
                            m_cee[0][jj] * m_cse[m_st][jj     ] +
                            m_cnn[0][jj] * m_cse[0 ][jj + m_st];
                }
            }

            if (m_st <= nx2)
            {
#pragma omp parallel for
                for (int ii = m_st; ii <= nx1; ii += m_tw)
                {
                    m_cse[ii][0] += m_cww[ii][0] * m_cse[ii - m_st][0 ] +
                            m_cee[ii][0] * m_cse[ii + m_st][0 ] +
                            m_cnn[ii][0] * m_cse[ii     ][m_st];
                }
            }
        } // master ----------------------------------------------------------------
#pragma omp parallel for
        for (int ii = m_tw; ii <= nx1; ii += m_tw)
        {
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                m_cse[ii][jj] +=  m_css[ii][jj] * m_cse[ii     ][jj - m_st] +
                        m_cww[ii][jj] * m_cse[ii - m_st][jj     ] +
                        m_cee[ii][jj] * m_cse[ii + m_st][jj     ] +
                        m_cnn[ii][jj] * m_cse[ii     ][jj + m_st];
            }
        }
        // for ---------------------------------------------------------------------
#pragma omp parallel for
        for (int ii = m_st; ii <= nx1; ii += m_tw)
        {
            for (int jj = m_tw; jj <= nx2; jj += m_tw)
            {
                m_cse[ii][jj] += m_css[ii][jj] * m_cse[ii     ][jj - m_st] +
                        m_cww[ii][jj] * m_cse[ii - m_st][jj     ] +
                        m_cee[ii][jj] * m_cse[ii + m_st][jj     ] +
                        m_cnn[ii][jj] * m_cse[ii     ][jj + m_st];
            }
        }
        // for ---------------------------------------------------------------------

        { // -----------------------------------------------------------------------
            if (!m_done2 && m_st <= 1)
            {
                m_done2 = true;
            }
        } // critical -------------------------------------------------------------

        if (!m_done2)
        {
            goto loop2;
        }

endloop2:; // while (m_st > 2)

        // note: the following is the same piece of code as the piece at the start of the preceding while loop
        /*
        { // -----------------------------------------------------------------------
            m_tw = m_st;
            m_st /= 2;
            m_niv -= 2;
            ASSERT(m_niv == 0,"not back where we started!");
        } // master ---------------------------------------------------------------

        for (int ii = m_st; ii <= nx1; ii += m_tw)
        {
            for (int jj = m_st; jj <= nx2; jj += m_tw)
            {
                m_cse[ii][jj] -= m_css[ii][jj] * m_cse[ii + m_st][jj - m_st] +
                        m_cww[ii][jj] * m_cse[ii - m_st][jj - m_st] +
                        m_cee[ii][jj] * m_cse[ii + m_st][jj + m_st] +
                        m_cnn[ii][jj] * m_cse[ii - m_st][jj + m_st];
            }
        }
        // for ---------------------------------------------------------------------

        // end of repeated code
*/

    } // master temporary?
} // solver


/// solves system
/** In deze subroutine wordt S(x)=R opgelost, waarbij S het
    Schurcomplement is dat ontstaat door alle
    zwarte punten van een schaakbordvolgorde te elimineren.
    We stoppen de iteratie zodra de 2-norm van het
    residu tenminste een factor tol is afgenomen.
    Het CG-algoritme is geimplementeerd zoals op blz.
    146 in 'Solving Linear Systems on Vector and Shared
    Memory Computers' van Dongarra, Duff, Sorensen en
    v.d. Vorst is beschreven.
    parameters:
    X: veld waarin de oplossing komt te staan.
    B: veld met rechterlid van het stelsel vergelijkingen.
       Deze vector wordt overschreven door het residu.  */
void RrbAkashSolver::solve(Array2D<REAL> *pB1, Array2D<REAL> *pB2, Array2D<REAL> *pX1, // real B(0:Nx1+2,0:Nx2+2)
                           Array2D<REAL> *pX2) // real X(0:Nx1+2,0:Nx2+2)
{

    Array2D<REAL> &Bz = *pB1;
    Array2D<REAL> &Xz = *pX1;

    Array2D<REAL> &Bf = *pB2;
    Array2D<REAL> &Xf = *pX2;

    /*
    Array2D<REAL> &cc = *m_pcc;
    Array2D<REAL> &cs = *m_pcs;
    Array2D<REAL> &cw = *m_pcw;

    const int nx1 = m_Nx1 + 1;
    const int nx2 = m_Nx2 + 1;
    const int nx12 = m_Nx1 + 2;
    const int nx22 = m_Nx2 + 2;
*/

    REAL bnrm2 = norm(Bz,Bf);

    int flag;

    if (bnrm2==0)
        bnrm2=1.0;


    // Compute the Residual r = B - A*X;
    Array2D<REAL> Rz(COLUMNS, ROWS);
    Array2D<REAL> Rf(COLUMNS, ROWS);
    // Compute A*X
    matvec_rrb(Xz,Xf,&Rz,&Rf);
    // Compute B-A*X
    Rz = MatSub(Bz,Rz);
    Rf = MatSub (Bf,Rf);

    REAL error_norm = norm(Rz,Rf) / bnrm2;

    std::vector<REAL> error_hist;
    error_hist.push_back(error_norm);

    m_done = error_norm > m_tolerance;

    if (!m_done) flag=0;
    int iter=0;

    REAL omega = 1.0;
    REAL beta=1.0;
    REAL rho =0.0, rho_1=0.0;
    REAL aleph=1.0;
    REAL norm_s=0.0;
    int max_iter=1000;

    // Can reduce the storage by reusing some variables- Later

    Array2D<REAL> Rz_tld(Rz);
    Array2D<REAL> Rf_tld(Rf);
    Array2D<REAL> Pz(Rz);
    Array2D<REAL> Pf(Rf);
    Array2D<REAL> Vz(Rz);
    Array2D<REAL> Vf(Rf);
    Array2D<REAL> Sz(Rz);
    Array2D<REAL> Sf(Rf);


    for (iter =1 ; iter< max_iter; ++iter)
    {
        rho = dotproduct(Rz_tld,Rz) + dotproduct(Rf_tld,Rf);

        if (rho==0)
        {
            flag=-1;
            break;
        }
        REAL wavetime,wavetime1;

        if (iter >1)
        {
            wavetime =seconds();
            beta = (rho*aleph)/(rho_1*omega);
            Pz = MatAdd(Rz, scalarmult(beta, MatSub(Pz,scalarmult(omega,Vz))));
            Pf = MatAdd(Rf, scalarmult(beta,MatSub(Pf,scalarmult(omega,Vf))));
            wavetime1 =seconds();

        }
        // Preconditioning Step ::
        Array2D<REAL> Pz_hat = Pz;
        Array2D<REAL> Pf_hat = Pf;

#ifdef prec
        prec_solve(Pz_hat, Pf_hat);
#endif
        //std::cout << "entered the solver" << std::endl;

        REAL wavetime2= seconds();

        //--------------------------------------------------

        matvec_rrb(Pz_hat, Pf_hat, &Vz, &Vf);


        aleph = rho / (dotproduct(Rz_tld,Vz) + dotproduct(Rf_tld,Vf));
        Sz = MatSub(Rz,scalarmult(aleph,Vz));
        Sf = MatSub(Rf,scalarmult(aleph,Vf));
        norm_s = norm(Sz,Sf);
        REAL resid =  norm_s /bnrm2;
        if (resid < m_tolerance)
        {
            flag=0;
            Xz = MatAdd(Xz,scalarmult(aleph,Pz_hat));
            Xf = MatAdd(Xf,scalarmult(aleph,Pf_hat));
            // REAL resid = norm_s /bnrm2;
            error_hist.push_back(resid);
            break;
        }

        REAL wavetime3 = seconds();

        // Preconditioning Step ::
        Array2D<REAL> Sz_hat(Sz);
        Array2D<REAL> Sf_hat(Sf);
#ifdef prec
prec_solve(Sz_hat,Sf_hat);
#endif

        REAL wavetime4 = seconds();
        //-----------------------------------------------------------------


        Array2D<REAL> Tz(Sz_hat);
        Array2D<REAL> Tf(Sf_hat);

        matvec_rrb(Sz_hat,Sf_hat,&Tz,&Tf);

        omega = (dotproduct(Tz,Sz) + dotproduct(Tf,Sf))/(dotproduct(Tz,Tz) + dotproduct(Tf,Tf));

        Xz = MatAdd(Xz,MatAdd(scalarmult(aleph,Pz_hat),scalarmult(omega,Sz_hat)));
        Xf = MatAdd(Xf,MatAdd(scalarmult(aleph,Pf_hat),scalarmult(omega,Sf_hat)));


        Rz = MatSub(Sz,scalarmult(omega,Tz));
        Rf = MatSub(Sf,scalarmult(omega,Tf));
        error_norm = norm(Rz,Rf)/bnrm2;
        REAL wavetime5 = seconds();

        REAL wavetime6 = (wavetime5-wavetime4) + (wavetime3-wavetime2) + (wavetime1-wavetime);
        REAL wavetime7 = (wavetime4-wavetime3) + (wavetime2-wavetime1);

        // std::cout << "Time for others:   " << wavetime6 << " \t Time for PC:   " << wavetime7 <<std::endl;

        error_hist.push_back(error_norm);
        if (error_norm < m_tolerance)
        {
            flag=0;
            break;
        }

        if (omega ==0.0)
        {
            flag=-2.0;
            break;
        }

        rho_1 = rho;
    }
    //std::string s1= "error" + std::to_string(i);

    // for(int i =0; i < error_hist.size() ;++ i)
     std::cout << iter << " " << error_norm << std::endl;


} // solve

void RrbAkashSolver::matvec_rrb(const Array2D<REAL> &X1, const Array2D<REAL> &X2 , Array2D<REAL> *Ax1, Array2D<REAL> *Ax2)
{
    Array2D<REAL> &Axz = *Ax1;
    Array2D<REAL> &Axf = *Ax2;

    Axz = MatAdd(matvec(m_ZetaZeta,X1),matvec(m_ZetaPhi,X2));
    Axf = MatAdd(matvec(m_PhiZeta,X1),matvec(m_PhiPhi,X2));
}

void RrbAkashSolver::prec_solve(Array2D<REAL> &Xz_hat, Array2D<REAL> &Xf_hat)
{
    //Array2D<REAL> Xz_hat = Xz;
    //Array2D<REAL> Xf_hat = Xf;
    solver(m_pZetaZeta, &Xz_hat);
    //Xf_hat = MatSub(Xf_hat,matvec(m_PhiZeta,Xz_hat));
    solver(m_pPhiPhi,&Xf_hat);

}

//Preconditioner 2
void RrbAkashSolver::prec_solve_2(Array2D<REAL> &Xz_hat, Array2D<REAL> &Xf_hat)
{

    solver(m_pPhiPhi,&Xf_hat);
    Xz_hat = MatSub(Xz_hat,matvec(m_ZetaPhi,Xf_hat));
    solver(m_pZetaZeta, &Xz_hat);
}

//Preconditioner 3 - Incomplete LU Type
void RrbAkashSolver::prec_solve_3(Array2D<REAL> &Xz_hat, Array2D<REAL> &Xf_hat)
{
    Array2D<REAL> Xz_hat1(Xz_hat);
    Array2D<REAL> Xf_hat1(Xf_hat);
    solver(m_pZetaZeta, &Xz_hat1);
    Xf_hat1 = MatSub(Xf_hat,matvec(m_PhiZeta,Xz_hat1));
    solver(m_pPhiPhi, &Xf_hat1);
    Array2D<REAL> temp(Xz_hat1);
    temp = matvec(m_ZetaPhi,Xf_hat);
    solver(m_pZetaZeta,&temp);
    Xz_hat = MatSub(Xz_hat1,temp);

}

inline void RrbAkashSolver::GeneratePlaneRotation (const REAL &dx, const REAL &dy, REAL *c1, REAL *s2)
{
    REAL &cs = *c1;
    REAL &sn=  *s2;

    if (dy == 0.0) {
        cs = 1.0;
        sn = 0.0;
    } else if (fabs(dy) > fabs(dx)) {
        REAL temp = dx / dy;
        sn = 1.0 / sqrt( 1.0 + temp*temp );
        cs = temp * sn;
    } else {
        REAL temp = dy / dx;
        cs = 1.0 / sqrt( 1.0 + temp*temp );
        sn = temp * cs;
    }
}

inline void RrbAkashSolver::ApplyPlaneRotation (REAL *dx1, REAL *dx2,const REAL &cs,const REAL &sn)
{
    REAL &dx = *dx1;
    REAL &dy=  *dx2;
    REAL temp  =  cs * dx + sn * dy;
    dy = -sn * dx + cs * dy;
    dx = temp;
}

inline void RrbAkashSolver::Update (Array2D<REAL> *X1, Array2D<REAL> *X2,
                                    int k, const Array2D<REAL> &h,
                                    const std::vector<REAL> &s,
                                    const std::vector<Array2D<REAL> > &Vz,
                                    const std::vector<Array2D<REAL> > &Vf)
{

    Array2D<REAL> &XZ = *X1;
    Array2D<REAL> &XF = *X2;


    std::vector<REAL> y(s);

    // Backsolve:
    for (int i = k; i >= 0; --i) {
        y[i] /= h[i][i];
        for (int j = i - 1; j >= 0; --j)
            y[j] -= h[j][i] * y[i];
    }

    for (int j = 0; j <= k; ++j)
    {
        XZ = MatAdd(XZ,scalarmult(y[j],Vz[j]));
        XF = MatAdd(XF,scalarmult(y[j],Vf[j]));
    }
}

void RrbAkashSolver::ComputeResidual(const Array2D<REAL> &Xz,const Array2D<REAL> &Xf,
                                     const Array2D<REAL> &Bz,const Array2D<REAL> &Bf,
                                     Array2D<REAL> *R1 , Array2D<REAL> *R2)
{
    Array2D<REAL> &Rz = *R1;
    Array2D<REAL> &Rf = *R2;

    // COMPUTE A*X and store in R
    matvec_rrb(Xz,Xf,&Rz,&Rf);
    // Compute B-A*X
    Rz = MatSub(Bz,Rz);
    Rf = MatSub (Bf,Rf);

    // Solve r = M\(B-A*X) where M is the preconditioner.

    prec_solve(Rz,Rf);

}

void RrbAkashSolver::GMRES(Array2D<REAL> *pB1, Array2D<REAL> *pB2, Array2D<REAL> *pX1, Array2D<REAL> *pX2)
{
    // Syntactic Sugar
    Array2D<REAL> &Bz = *pB1;
    Array2D<REAL> &Xz = *pX1;

    Array2D<REAL> &Bf = *pB2;
    Array2D<REAL> &Xf = *pX2;


    Array2D<REAL> Rz(COLUMNS, ROWS);
    Array2D<REAL> Rf(COLUMNS, ROWS);
    int i,k, flag;
    Rz=Bz;
    Rf=Bf;

    prec_solve(Rz,Rf);
    REAL bnrm2 = norm(Rz,Rf);

    if (bnrm2==0) bnrm2=1.0;

    // R = M\ (B-AX)
    Rz=0.0;
    Rf=0.0;
    ComputeResidual(Xz,Xf,Bz,Bf,&Rz,&Rf);

    REAL beta = norm(Rz,Rf);
    REAL error_norm = beta / bnrm2;
    if (error_norm < m_tolerance)
    {
        flag = 2.0;
        std::cout << "Converged without iteration" << std::endl;
        return;
    }

    // ------------ GMRES Solver Parameters

    int m = 30; // The restart variable of GMRES
    int max_iter=100;
    int iter=0;
    Array2D<REAL> H(m+1,m);
    //REAL H[m+1][m];
    std::vector<REAL> s(m+1,0), cs(m+1,0), sn(m+1,0);
    std::vector< Array2D<REAL> > Vz, Vf;
    //   / Array2D<REAL> w , av;


    for (iter =1; iter <max_iter ; ++iter)
    {
        flag=0;
        //ComputeResidual(Xz,Xf,Bz,Bf,&Rz,&Rf);

      //  REAL norm_r = norm(Rz,Rf);
        REAL norm_r1 = 1/beta;
        Vz.clear();
        Vf.clear();
        Vz.push_back(scalarmult(norm_r1,Rz));
        Vf.push_back(scalarmult(norm_r1,Rf));
        // Need to check the definition of Sz, and Sf, and where it is used!!
        //  Array2D<REAL> Sz = scalarmult(norm(Rz),e1);
        // Array2D<REAL> Sf = scalarmult(norm(Rf),e1);
        s.assign(m+1,0);
        s[0] = beta;
        for (i =0 ; i<m ; ++i)
        {
            Array2D<REAL> Wz, Wf;
            matvec_rrb(Vz[i],Vf[i],&Wz,&Wf);
            prec_solve(Wz,Wf);
            for (k =0 ; k<=i ; ++k)
            {
                H[k][i] = dotproduct(Wz,Vz[k]) + dotproduct(Wf,Vf[k]);
                Wz = MatSub(Wz,scalarmult(H[k][i] , Vz[k]));
                Wf = MatSub(Wf,scalarmult(H[k][i] , Vf[k]));

            }
            H[i+1][i]= norm(Wz,Wf);
            norm_r1= 1/H[i+1][i];
            Vz.push_back(scalarmult(norm_r1,Wz));
            Vf.push_back(scalarmult(norm_r1,Wf));
            for (k =0 ; k<i ; ++k)
            {
//                REAL temp =


                // Given Rotation
                ApplyPlaneRotation(&H[k][i],& H[k+1][i], cs[k], sn[k]);
            }

            GeneratePlaneRotation(H[i][i], H[i+1][i], &cs[i], &sn[i]);
            ApplyPlaneRotation(&H[i][i], &H[i+1][i], cs[i], sn[i]);
            ApplyPlaneRotation(&s[i], &s[i+1], cs[i], sn[i]);
            error_norm = fabs(s[i+1])/bnrm2;

            if (error_norm<m_tolerance)
            {
                Update(&Xz, &Xf, i, H, s, Vz, Vf);
                Vz.clear();
                Vf.clear();
                flag=1;
                break;
                //return;
            }
        }
        if (flag==1) break;

        Update(&Xz, &Xf, i-1, H, s, Vz, Vf);
        ComputeResidual(Xz,Xf,Bz,Bf,&Rz,&Rf);
        beta = norm(Rz,Rf);
      //  s[i+1] = norm(Rz,Rf);
        error_norm = beta/bnrm2;
        if (error_norm<m_tolerance)
        {
            Vz.clear();
            Vf.clear();
            flag=2;
            break;
        }
        //return;
    }


    std::cout << "Error is" << error_norm << "\tIterations are: " << iter  << " Inner iterations are" << i<< "\t Flag is : " <<flag <<std::endl;
}
